#include "Command.h"
#include "malloc.h"
#include "malloc.h"
#include "UserMotor.h"
extern Mallco_dev mallco_dev; //内存管理器


/**********************************
 * 功能：回复数据
 * 参数：
 *         protocol：协议
 * 返回：回复数据
 * *******************************/
JA_BytesArray Reply_Data(JA_Protocol protocol)
{
	JA_BytesArray buf;
	buf.bytes = mymalloc(&mallco_dev, protocol.data.data_len + 8);
	if (buf.bytes)
	{
		buf.len = JA_Package(protocol, buf.bytes);
		return buf;
	}
}

/*********************************
 * 功能：设置运动速度
 * 参数：
 *         protocol：协议
 * 返回：回复数据
 * ******************************/
JA_BytesArray MotorRun(JA_Protocol protocol)
{
	EC_I_30_Run_t run_info;
	JA_Protocol r_protocol;
	uint8_t status_code = 0;
	mymemcpy((uint8_t *)&run_info, protocol.data.data, protocol.data.data_len);
	SetMotorRun(run_info.model,run_info.value);
	r_protocol.Address=protocol.Address;
	r_protocol.CS_Vsersion=protocol.CS_Vsersion;
	r_protocol.SN=protocol.SN;
	r_protocol.data.data_len = 1;
	r_protocol.data.Opcode = (uint8_t)Motor_Rotate;
	r_protocol.data.data = &status_code;
	return Reply_Data(r_protocol);
}
JA_BytesArray QueryInfo(JA_Protocol protocol)
{
	Command_info_t info;
	info.software_version.build=APP_VERSION_Build;
	info.software_version.major=APP_VERSION_Major;
	info.software_version.minor=App_VERSION_Minor;
	info.software_version.revision=APP_VERSION_Revision;
	info.dev_type=model_info_handle.model_type;
	info.solidify.SN=model_info_handle.general_info.SN;
	JA_Protocol r_protocol;
	uint8_t status_code = 0;
	r_protocol.Address=protocol.Address;
	r_protocol.CS_Vsersion=protocol.CS_Vsersion;
	r_protocol.SN=protocol.SN;
	r_protocol.data.Opcode = (uint8_t)ConfigCommand_GetInfo;
	r_protocol.data.data = (uint8_t *)&info;
	r_protocol.data.data_len=sizeof(Command_info_t);
	return Reply_Data(r_protocol);
}

/*********************************
* 功能：查询电机运行状态
 * 参数：
 *         protocol：协议
 * 返回：回复数据
 * ******************************/
JA_BytesArray QueryMotorRunState(JA_Protocol protocol)
{
	Command_MotorRunState_t run_state;
	run_state.velocity=GetMotorSpeed();
	run_state.angle=GetMotorAngle();
	run_state.state=0;
	JA_Protocol r_protocol;
	r_protocol.Address=protocol.Address;
	r_protocol.CS_Vsersion=protocol.CS_Vsersion;
	r_protocol.SN=protocol.SN;
	r_protocol.data.data_len = 1;
	r_protocol.data.Opcode = (uint8_t)Motor_QueryRunState;
	r_protocol.data.data = (uint8_t *)&run_state;
	r_protocol.data.data_len=sizeof(Command_MotorRunState_t);
	return Reply_Data(r_protocol);
}
/*********************************
 * 功能：查询PID参数
 * 参数：
 *         protocol：协议
 * 返回：回复数据
 * ******************************/
JA_BytesArray QueryMotorPID(JA_Protocol protocol)
{
	Command_PID_t pid;
	pid.velocity=GetPID_Velocity();
	pid.angle=GetPID_Angle();
	pid.current=GetPID_Current();
	JA_Protocol r_protocol;
	r_protocol.Address=protocol.Address;
	r_protocol.CS_Vsersion=protocol.CS_Vsersion;
	r_protocol.SN=protocol.SN;
	r_protocol.data.data_len = 1;
	r_protocol.data.Opcode = (uint8_t)Motor_QueryPID;
	r_protocol.data.data = (uint8_t *)&pid;
	r_protocol.data.data_len=sizeof(Command_PID_t);
	return Reply_Data(r_protocol);
}
/*********************************
 * 功能：设置速度PID
 * 参数：
 *         protocol：协议
 * 返回：回复数据
 * ******************************/
JA_BytesArray Command_SetPID_Velocity(JA_Protocol protocol)
{
	PID_t pid;
	JA_Protocol r_protocol;
	uint8_t status_code = 0;
	mymemcpy((uint8_t *)&pid, protocol.data.data, protocol.data.data_len);
	SetPID_Velocity(pid);
	r_protocol.Address=protocol.Address;
	r_protocol.CS_Vsersion=protocol.CS_Vsersion;
	r_protocol.SN=protocol.SN;
	r_protocol.data.data_len = 1;
	r_protocol.data.Opcode = (uint8_t)Motor_SetPID_Velocity;
	r_protocol.data.data = &status_code;
	return Reply_Data(r_protocol);
}
/**********************************
*功能：JA协议解析函数
*参数：
			protocol：协议消息数据
*返回：无
**********************************/
JA_BytesArray JA_MotorParse(JA_Protocol protocol, uint8_t *command_buf)
{
	JA_BytesArray reply_buf;
	reply_buf.len = 0;
	reply_buf.bytes = NULL;
	uint8_t state_code = 0;
	switch (protocol.data.Opcode)
	{
		case (uint8_t)ConfigCommand_GetInfo:
			reply_buf=QueryInfo(protocol);
			break;
		case (uint8_t) Motor_QueryRunState:
			reply_buf=QueryMotorRunState(protocol);
			break;
	case (uint8_t)Motor_Rotate: //控制电机运动
		reply_buf=MotorRun(protocol);
		break;
	case (uint8_t)Motor_QueryPID:  //查询PID参数
		reply_buf=QueryMotorPID(protocol);
		break;
	case (uint8_t)Motor_SetPID_Velocity:
		reply_buf=Command_SetPID_Velocity(protocol);
		break;
	case (uint8_t)Motor_Abolish: //取消一切动作
		break;
	case (uint8_t)Motor_GoLocation: //指定位置
		break;
	    break;
	default:
		break;
	}
	return reply_buf;
}